First, a prior known floating mobile object control technique is described with reference to FIGS. 11 through 14 and so on. FIG. 11 is a schematic diagram of a prior known floating mobile object, and FIGS. 11 through 14 are schematic diagrams illustrating a prior known floating mobile object control technique.
As shown in FIG. 11, in order to control the position and velocity of the main body of a floating mobile object 10 such as an underwater robot, the main body is provided with a sensor 13 by which the position and velocity of the main body of the floating mobile object 10 are sensed and fed to a subtraction circuit 11 in a computer P. The subtraction circuit 11 is also fed with a signal indicating desired position and velocity from an operator H. An output of the subtraction circuit 11 is provided to a thruster control circuit 12 for controlling thrusters T provided as thrust generation means (effectors) on the floating mobile object 10, thereby controlling the position and velocity of the floating mobile object 10 (which is what is called position/velocity feedback control).
FIG. 12 is a block diagram for controlling the floating mobile object 10 shown in FIG. 11. An effector thrust command signal, which is an output of the thruster control circuit 12, is fed to the thrusters T having a predetermined thrust characteristic 14, thereby feeding an effector thrust to the floating mobile object 10, so that dynamics of the floating mobile object 10, i.e., dynamic input/output characteristics, are obtained as denoted by reference character 15, and the position and velocity of the main body is sensed by the sensor 13 and fed to the subtraction circuit 11 as described above.
As such, in the conventional control of a floating mobile object, information concerning the position and velocity of a floating mobile object, which is a subject of control, is fed back to directly handle a thrust command to an effector, and in the case of a floating mobile object generally having an effector with a relatively slow response speed, it takes a time while after the command is sent to the effector, the effect of which is reflected in the position and velocity of the floating mobile object, until the sensor obtains them, therefore there arises a problem where control performance is adversely affected (which is what is called a sensing delay problem). This tendency becomes noticeable as the mass of the floating mobile object is increased or the thrust of the effector is decreased.
Further, a floating mobile object in a stream of water or air is always affected by hydrodynamic disturbances such as waves and tidal current or wind, and in addition, as shown in FIG. 11, the floating mobile object 10 in water is connected with an umbilical cable C (a feeder, a communication line or the like) from above the water, which often pulls the floating mobile object 10, therefore in the case of the above-described control technique, a response to unknown disturbances takes time, causing a problem where control performance is adversely affected.
Next, in the case where the floating mobile object 10 is further provided with a robot arm A having a plurality of links as shown in FIG. 11, a block diagram as illustrated in FIG. 13 is constructed. A reaction force and torque by the robot arm A as denoted by reference character 16 is added to effector thrusts by the thrusters T as illustrated by an adding circuit 17. Accordingly, the reaction force and torque from the robot arm A disturbs the action of the floating mobile object 10, i.e., the position and velocity, and the thrusters T are controlled to correct errors in the position and velocity of the floating mobile object 10 that are caused by the reaction force and torque of the robot arm A. As such, the action (position and velocity) of the floating mobile object 10 is disturbed by the reaction force and torque from the robot arm A, and therefore conventional attitude control activates the thrusters T so as to correct the resulting errors in the position and velocity.
Specifically, the prior known control technique operates activates the thrusters T after the floating mobile object 10 is moved by the reaction force and torque of the robot arm A, and therefore the so-called sensing delay problem where a response speed to disturbances is low still remains unsolved.
FIG. 14 illustrates a block diagram of another conventional art. This control technique takes into consideration mutual interference between the motion of the robot arm A and the motion of the main body of the floating mobile object 10, and simultaneously determines an effector thrust and a robot arm joint torque, such that their motions are consistent with their respective purposes. In this conventional art, the floating mobile object 10 provided with the robot arm A is configured as a multi-link system, and based on its equation of motion, influence of each joint torque of each robot arm on the motion of the main body of the floating mobile object 10 is expressed in a mathematical formula, which is solved to determine propulsion power of the main body of the floating mobile object 10 (which is what is called model-based control).
In the conventional art of FIG. 14, an effector thrust command signal from a thruster/robot arm control circuit 18 is fed to the thrusters T, and a torque command signal of the robot arm A is fed to a joint actuator 19. As such, complex system dynamics 20 of the main body of the floating mobile object 10 and the robot arm A are formed by effector thrusts outputted from the thrusters T and a joint torque by the joint actuator 19, and the sensor 13 senses the position and velocity of the main body of the floating mobile object 10 and feeds them to the subtracter 11 to which a signal representing desired position and attitude of the main body of the floating mobile object 10 is fed. Also, robot arm control variables from the complex system dynamics 20, i.e., signals representing the position, attitude, joint angle, velocity, hand reaction force, etc., are fed to the subtraction circuit 21 to which target values for the control variables of the robot arm A are fed. Outputs of the subtracters 11 and 21 are fed to the thruster/robot arm control circuit 18.
Specifically, this control technique takes into consideration the mutual interference between the motion of the robot arm A and the motion of the main body of the floating mobile object 10, and simultaneously determines an effector thrust and a robot arm joint torque, such that their motions are consistent with their respective purposes. While this advantageously allows the motion of the main body of the floating mobile object 10 to be accurately controlled, there are the following problems (1) through (3).
(1) The object dynamics are generally complicated, and if the robot arm A has six axes, the dynamics take a form of a link joining seven rigid bodies including the main body of the floating mobile object 10. In the case where there are a plurality of robot arms A, the dynamics become more significantly complicated. Arithmetic of such control system is considerably time-consuming, and imposes a heavy computation load on the computer. Therefore, it is practically very difficult to configure the above-described control system with a small computer.
(2) Also, the above-described control system requires a number of dynamic parameters such as the mass, moment of inertia and gravitational center position of each link of the robot arm A and the main body, and proper control cannot be realized by using these dynamic parameters unless their values are all correct. Accordingly, in the case of holding an object with the robot arm A, the mass, moment of inertia and gravitational center position of the held object should be provided appropriately. Thus, it is required to have a database of objects that can be held or measure dynamic characteristics of the object, therefore there arises a problem where the use is practically limited.
(3) Further, a floating mobile object in a stream of water or air is always affected by unknown hydrodynamic disturbances such as waves and tidal current or wind, and in addition, as shown in FIG. 11, the floating mobile object 10 in water is connected with an umbilical cable C (a feeder, a communication line or the like) from above the water, which often pulls the floating mobile object 10, besides it is not possible to model the unknown disturbances, and in addition it is difficult to predict influence of the disturbances, therefore such a model-based control technique is disadvantageous in that it cannot be fundamentally adapted to control of a floating mobile object.
Other prior known floating mobile object control techniques include, for example, feedback control of force at a joint of a manipulator as disclosed in Patent Document 1 and local feedback control of a thrust as disclosed in Non-patent Document 1.
According to the feedback control of a force at a joint of a manipulator in Patent Document 1, it is possible to activate thrusters before the main body of a floating mobile object is moved due to disturbances by the manipulator, thereby canceling any influence on the main body of the floating mobile object and preventing the main body of the floating mobile object from being moved by the disturbances by the manipulator.
Also, the thrusters used as effectors for controlling the position/velocity of the floating mobile object generates a thrust by taking advantage of a stream, and therefore a high nonlinearity caused by the stream is present between an input command to the thrusters and a thrust that is actually outputted. Such a nonlinearity results from various factors, and conventionally it is difficult to perform control with consideration of all of them (a problem of what is called a nonlinearity of an effector dynamic characteristic). However, according to the local feedback control of a thrust in Non-Patent Document 1, it is possible to prevent control performance for the floating mobile object from deteriorating due to the nonlinearity of effector dynamic characteristic.
While both of the above-described control techniques according to Patent Document 1 and Non-Patent Document 1 measure and feed back force partially, they do not cancel influence of waves and tidal current or wind which are the greatest disturbances to the floating mobile object.
In brief, neither of the above-described conventional arts realizes a floating mobile object control system capable of automatically compensating for disturbing influence even under disturbances caused by reaction, etc., of a robot arm and influence of unknown hydrodynamic disturbances such as waves and tidal current or wind to which a floating mobile object in water, air or the like is always subjected, thereby allowing the floating mobile object to stand still with high precision or track a target trajectory with high precision.
On the other hand, aside from the above-described problems, there is a below-described problem with inertial navigation, which is a prior known technique widely used for navigating a floating mobile object. Specifically, the inertial navigation uses an acceleration obtained from a floating mobile object, and in the case where the floating mobile object itself is large and heavy, a prior known acceleration sensor, which temporally converts an acceleration to a displacement and outputs the value of acceleration as an electric signal, is structurally unable to sense a minor change in the acceleration with high precision, therefore there arises a problem where a noticeable error is present between the acceleration value obtained from the sensor and an actual acceleration value. Such an error between the acceleration values considerably affects the accuracy of the inertial navigation itself, causing inconvenience of being unable to accurately guide the floating mobile object onto a desired trajectory.
[Patent Document 1] Japanese Laid-Open Patent Publication No. 5-119837
[Non-Patent Document 1] Kanaoka, Nakayama, Hayashi and Kawamura, “Thrust Local Feed-Back Control for Realization of High Speed and Precise Motion in Water”, proceedings of the Robotics and Mechatronics Conference '03, the Japan Society of Mechanical Engineers, 2P1-2F-A6, 2003